#ifndef _OBSTACLEDETECTOR_H
#define _OBSTACLEDETECTOR_H

#include <iostream>
#include <memory>
#include <cstdint>
#include <Eigen/Dense>
#include <string>
#include <lanelet2_io/Io.h>
#include <lanelet2_projection/UTM.h>

#include "math/basic_math.h"
#include "module_base/util.h"
#include "pc_conversion/pc_conversion.h"
#include "bevfusion/bevfusion.hpp"

#include "obstacle_detector/pointcloud_filter.h"
#include "obstacle_detector/debug_display.h"
#include "obstacle_detector/util.h"
#include "obstacle_detector/map_processor.h"
#include "obstacle_detector/bbox_processor.h"
#include "obstacle_detector/trajectory_predictor.h"
#include "obstacle_detector/collision_detector.h"

namespace obstacle_detector
{
class ObstacleDetector
{
public:
	ObstacleDetector(std::string calib_folder,std::string weight_folder,
		bool debug);
	~ObstacleDetector(){};
	void set_pcdata(const uint8_t *data1, const uint8_t *data2, int point_num1,
		int point_num2,double stamp);
	void set_filter_pcdata(const uint8_t *data,int point_num);
	void set_map(std::string map_path);
	void set_vechile_param(const module_base::VehicleParam &vehicle_param);
	void set_control_sequence(const float *control_sequence);
	float check_collision(module_base::Location &loc);
	const std::vector<Object>& get_obstacles(); 

private:
	void motion_prediction(const float* control_sequence,
		float *state);

private:
	const int DT=50;
	bool debug_;

	int16_t *origin_points_gpu_;
	float *points_gpu_;
	float *timestamp_gpu_;
	int points_num_;

	int16_t *filtered_points_gpu_;
	uint16_t *points_order_gpu_;
	uint8_t *point_road_index_gpu_;
	int filtered_points_num_;

	bool map_init_;
	bool vehicleparam_init_;

	module_base::Location loc_;

	module_base::VehicleParam vehicle_param_;

	std::vector<Object> lst_objects_;

	std::unique_ptr<pc_conversion::PCConversion> pc_conversion_;
	std::unique_ptr<DebugDisplay> debug_display_;
	std::unique_ptr<PointCloudFilter> pointcloud_filter_;
	std::unique_ptr<MapProcessor> map_processor_;
	std::unique_ptr<BBoxPorcessor> bbox_processor_;
	std::unique_ptr<TrajectoryPredictor> trajectory_predictor_;
	std::unique_ptr<CollisionDetector> collision_detector_;

	std::shared_ptr<bevfusion::Core> detector_;
};

}//namespace

#endif